#ifndef ROSGUIAPI_H
#define ROSGUIAPI_H
#include <string>
#include <vector>
#include <ros/ros.h>
#include <math.h>
#include <vector>
#include <sstream>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <actionlib_msgs/GoalID.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <sys/stat.h>
#include <unistd.h>
#include <stdlib.h>
#include <tf/transform_datatypes.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/UInt32MultiArray.h>
#include <tcr100/VehicleEncoder.h>
#include <tcr100/VehicleGoal.h>
#include "ros/master.h"
#include "ros/this_node.h"

using namespace std;
#include "gSoapFoundation/rosgsoapnamespaceStub.h"
using namespace rosgsoapnamespace;
//struct UserInfo
//{
//    int account;                    //用户ID
//    std::string password;           //密码
//    std::string ip;                 //IP地址
//    std::string operationSystem;    //操作系统
//};

//struct VehiclePosition
//{
//    double x;//m
//    double y;//m
//    double rotate;//unit is rad/s
//};

//enum E_MAP_SOLVER
//{
//    E_MAP_SOLVER_GMAPPING,
//    E_MAP_SOLVER_CARTOGRAPHER,
//    E_MAP_SOLVER_HECTOR
//};

//enum E_LOCATION_SOLVER
//{
//    E_LOCATION_SOLVER_AMCL,
//    E_LOCATION_SOLVER_CARTOGRAPHER_LASER,
//    E_LOCATION_SOLVER_CARTOGRAPHER_LASER_IMU

//};
//enum E_ODOM_SOLVER
//{
//    E_ODOM_SOLVER_ENCODER,
//    E_ODOM_SOLVER_LASER,
//    E_ODOM_SOLVER_ENCODER_IMU,
//    E_ODOM_SOLVER_LASER_ENCODER_IMU
//};

//enum E_TRAJECTORY_GLOBAL_SOLVER
//{
//    E_TRAJECTORY_GLOBAL_SOLVER_ASTAR,
//    E_TRAJECTORY_GLOBAL_SOLVER_DIJKSTRAS
//};

//enum E_TRAJECTORY_LOCAL_SOLVER
//{
//    E_TRAJECTORY_LOCAL_SOLVER_DWA,
//    E_TRAJECTORY_LOCAL_SOLVER_BASELOCAL,
//    E_TRAJECTORY_LOCAL_SOLVER_TEB
//};
//enum E_GOAL_STATUS
//{
//    E_GOAL_STATUS_PENDING,
//    E_GOAL_STATUS_ACTIVE,
//    E_GOAL_STATUS_PREEMPTED,
//    E_GOAL_STATUS_SUCCEEDED,
//    E_GOAL_STATUS_ABORTED,
//    E_GOAL_STATUS_REJECTED,
//    E_GOAL_STATUS_PREEMPTING,
//    E_GOAL_STATUS_RECALLING,
//    E_GOAL_STATUS_RECALLED,
//    E_GOAL_STATUS_LOST,

//};

//enum E_ROS_PACKAGE_STATUS
//{
//    E_ROS_PACKAGE_STATUS_DOWN,
//    E_ROS_PACKAGE_STATUS_NORMAL,
//    E_ROS_PACKAGE_STATUS_ERROR
//};

class RosGuiApi
{
public:
    RosGuiApi();
    ~RosGuiApi();
public:
    //***************接口函数部分********************************

    //地图
    bool init();
    int setMapSolver(struct UserInfo userInfo,enum E_MAP_SOLVER solverIn,int laserCount,int &returnFlag);//建图选择 enum 建图方法 int 雷达数量 int 返回标志
    int createMapStart(struct UserInfo userInfo,int &returnFlag);//开始建图 string 地图名称 int 返回标志
    int createMapEnd(struct UserInfo userInfo,std::string mapName,int &returnFlag);//结束建图 string 地图名称 int 返回标志
    int loadMap(struct UserInfo userInfo,std::string mapName,int &returnFlag);//加载建图 string 地图名称 int 返回标志


//    //添加虚拟墙
    int addVitrualBlock(struct UserInfo userInfo,std::string configFilePath,int &returnFlag);
    int deleteVitrualBlock(struct UserInfo userInfo,int &returnFlag);
//    int addForbidenZone(struct UserInfo userInfo,std::vector<double>point_x,std::vector<double>point_y,int &returnFlag);

//    //定位
    int setOdomSolver(struct UserInfo userInfo,enum E_ODOM_SOLVER solverIn,int &returnFlag);//选择odom enum 方法 int 返回标志
    int setLocationSolver(struct UserInfo userInfo,enum E_LOCATION_SOLVER solverIn,int &returnFlag);//选择定位算法 enum 定位方法 int 返回标志
    int getVehiclePosition(struct UserInfo userInfo,struct VehiclePosition& positionReturn);//获取agv位置 struct 位位置

    int setVehiclePosition(struct UserInfo userInfo,struct VehiclePosition positionIn,int &returnFlag);//设置agv初始位置 struct 位位置 int 返回标志

//    //轨迹规划和行驶
    int setTrajectoryGlobalPlanSolver(struct UserInfo userInfo,enum E_TRAJECTORY_GLOBAL_SOLVER solverIn,int &returnFlag);//选择全局规划器 enum 全局规划器 int 返回标志
    int setTrajectoryLocalPlanSolver(struct UserInfo userInfo,enum E_TRAJECTORY_LOCAL_SOLVER solverIn,int &returnFlag);//选择局部规划器 enum 局部规划器 int 返回标志
    int setReplanCoutLimitWhenFailure(struct UserInfo userInfo,int countLimit,int &returnFlag);//设置失败重试次数 int 次数

    int startNavigation(struct UserInfo userInfo,int &returnFlag);//start navigation
    int stopNavigation(struct UserInfo userInfo,int &returnFlag);//stop navigation

    int moveToPosition(struct UserInfo userInfo,struct VehiclePosition positionIn,double velocity,int &returnFlag);//M/S  导航到指定点 struct 目标位置 double 车辆速度 int 返回标志
    int moveFollowPath(struct UserInfo userInfo,std::string filePath,std::string trajectoryName,double nearbyDistance,double velocity,int &returnFlag);//多点巡航  std::vector<struct VehiclePosition> 路径 double 速度
    int changeVelocity(struct UserInfo userInfo,double velocity,int &returnFlag);//设置速度 double 速度 int 返回标志
    int getMovingStatus(struct UserInfo userInfo,enum E_GOAL_STATUS& status);//获取导航状态 enum 状态
    int getRemainningTime(struct UserInfo userInfo,double &timeOut);//获取剩余时间  double 剩余时间
    int getRemainningDistance(struct UserInfo userInfo,double &distanceOut);//获取剩余距离  double 剩余距离
    int cancelMotion(struct UserInfo userInfo,int &returnFlag);//取消当前导航目标 int 返回标志

//    //设置
    int setPositionArrivedAccuracy(struct UserInfo userInfo,double distanceErrorIn,double angularErrorIn,int &returnFlag);//设置导航允许误差 double xy距离误差 double 航向误差 int 返回标志


    int setLocalPlanRange(struct UserInfo userInfo,double x_rangeIn,double y_rangeIn,int &returnFlag);//设置局部地图大小 double 长 double 宽
    int setVehicleBorder(struct UserInfo userInfo,std::vector<double>point_x,std::vector<double>point_y,int &returnFlag);//设置车体的footprint std::vector<double>x坐标 std::vector<double>y坐标
    int setLocalBarrierOffset(struct UserInfo userInfo,double offsetIn,int &returnFlag); //设置膨胀区大小 double 膨胀距离 int 返回标志
    int setGlobalBarrierOffset(struct UserInfo userInfo,double offsetIn,int &returnFlag); //设置膨胀区大小 double 膨胀距离 int 返回标志
    int startPathFollowMotion(UserInfo userInfo,int &returnFlag);
    int ceaseMotion(UserInfo userInfo,int &returnFlag);
    int startMotion(UserInfo userInfo,int &returnFlag);

//    int resetAll(struct UserInfo userInfo,int &returnFlag);//重置
//    //todo 规划器的参数

//    //状态
//    int getRosPackageStatus(enum E_ROS_PACKAGE package,enum E_ROS_PACKAGE_STATUS &status);//获取ROS包的状态 enum 状态


    int setWheelOdometerData(UserInfo userInfo,int robotIdIn,struct OdemeterData odemeterDataReturn,int &returnFlag);

    int getVehicleVelocityComand(UserInfo userInfo,int robotIdIn,struct  VehicleVelocity &velocityComamnd);
    void freeSleep();

private:
    void moveBaseResultCallback(const actionlib_msgs::GoalStatusArray::ConstPtr& msg);
    void twistCmdVelCallback(const geometry_msgs::Twist &msg);
    int setVelocity(double velocity);//set velocity
    int setNearbyDistance(double distance);//set nearby distance
    bool moveToGoal(struct VehiclePosition positionIn);
    void gloablPathLenghtCallback(const nav_msgs::Path& path);
    void movebaseGoalCallback(const move_base_msgs::MoveBaseActionGoal& goal);
    int getPositionFromROS();
    void robotPoseCallback(const geometry_msgs::PoseWithCovarianceStamped& pose);
private:
    int map_selector;
    int localization_algorithm;
    int local_trajectory_selector;
    int global_trajectory_selector;
    int nav_status;
    const char* map_to_load;
    double path_lenght;

    ros::NodeHandle nh_;
    ros::Rate rate_;

    move_base_msgs::MoveBaseGoal current_goal;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client_;
    tf::TransformListener listener_;
    tf::StampedTransform transform_;


    ros::Publisher initial_position_pub_;
    ros::Publisher goal_pub_;
    ros::Subscriber move_base_status_sub_;
    ros::Subscriber move_base_path_sub_;
    ros::Subscriber move_base_goal_sub_;
    ros::Subscriber cmd_vel_;
    ros::Publisher encoder_pub_;
    ros::Subscriber pose_sub_;
    VehicleVelocity rosVehicleCmdVel;
    VehiclePosition rosVehiclePosition;


};

#endif // ROSGUIAPI_H
